import numpy as np
import casadi as ca

from .model import Model


class Omron(Model):

    def __init__(self):
        super().__init__()

        self._num_state = 3
        self._num_control = 2

        self.radius = 0.35
        self.v_max = 1.0
        self.omega_max = np.pi / 5.0

        self._limit = np.array([
            [-self.v_max, self.v_max],
            [-self.omega_max, self.omega_max]
        ])

    @staticmethod
    def casadi_derivatives(x: np.ndarray, u: np.ndarray):
        return ca.vertcat(u[0] * ca.cos(x[2]), u[0] * ca.sin(x[2]), u[1])
